/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "Car.h"
#include "owndebug.h"
#include "World.h"
//#include "Car_settings.h"

CObject* CCar::LoadMLMA(CWorld *world, xmlNode *node)
{
  CObject *object=NULL;
  // Parameters for a Line.
  xmlChar *x = xmlGetProp(node, (xmlChar *) "x");
  xmlChar *y = xmlGetProp(node, (xmlChar *) "y");
  xmlChar *heading = xmlGetProp(node, (xmlChar *) "heading");
  xmlChar *name = xmlGetProp(node, (xmlChar *) "name");
  xmlChar *group = xmlGetProp(node, (xmlChar *) "MaCIGroup");
  bool driverCam=false, boomCam=false, headCam=false;
  
  float boomLength=BOOM_LENGTH, boomHeight=BOOM_CAMERA_HEIGHT, boomFOV=BOOM_CAMERA_FOV;
  
  if (xmlGetProp(node,(xmlChar *)"driver_camera"))
  {
    if (!(xmlStrcmp(xmlGetProp(node,(xmlChar *)"driver_camera"),(xmlChar*)"enabled"))
        || !(xmlStrcmp(xmlGetProp(node,(xmlChar *)"driver_camera"),(xmlChar*)"enable")))
    {
      driverCam=true;
      dPrint(1,"Using driver camera in Car!");
    }
  }
  
  if (xmlGetProp(node,(xmlChar *)"boom_camera"))
  {
    if (!(xmlStrcmp(xmlGetProp(node,(xmlChar *)"boom_camera"),(xmlChar*)"enabled"))
        || !(xmlStrcmp(xmlGetProp(node,(xmlChar *)"boom_camera"),(xmlChar*)"enable")))
    {
      boomCam=true;
      dPrint(1,"Using boom camera in Car!");
      if (xmlGetProp(node,(xmlChar *)"boom_length"))
      {
        //dPrint(1,"boom length tag found");
        xmlChar *c=xmlGetProp(node, (xmlChar *) "boom_length");
        if (c)
        {
          boomLength=atof((const char*)c);
          //dPrint(1,"boom length set %f",boomLength);
          xmlFree(c);
        }
      }
      if (xmlGetProp(node,(xmlChar *)"boom_height"))
      {
        xmlChar *c=xmlGetProp(node, (xmlChar *) "boom_height");
        if (c)
        {
          boomHeight=atof((const char*)c);
          xmlFree(c);
        }
      }
      if (xmlGetProp(node,(xmlChar *)"boom_fov"))
      {
        xmlChar *c=xmlGetProp(node, (xmlChar *) "boom_fov");
        if (c)
        {
          boomFOV=atof((const char*)c);
          xmlFree(c);
        }
      }
    }
  }
  
  if (xmlGetProp(node,(xmlChar *)"head_camera"))
  {
    if (!(xmlStrcmp(xmlGetProp(node,(xmlChar *)"head_camera"),(xmlChar*)"enabled"))
        || !(xmlStrcmp(xmlGetProp(node,(xmlChar *)"head_camera"),(xmlChar*)"enable")))
    {
      headCam=true;
      dPrint(1,"Using head camera in Car!");
    }
  }
  
  // Process. 
  if (x && y && heading && (name || group)) {
    object=new CCar(world,
                    atof((const char*)x),
                    atof((const char*)y),
                    atof((const char*)heading),
                    name!=NULL ? (const char*)name : "",
                    group!=NULL ? (const char*)group : "",
                    driverCam, headCam, boomCam,
                    boomFOV, boomLength, boomHeight);
  } else {
    dPrint(1,"MLMA Error:Not enough parameters defined for Car-object!");
  }

  // Cleanup (Always do this to clean out properties which were found)
  if (x) xmlFree(x);
  if (y) xmlFree(y);
  if (heading) xmlFree(heading);
  if (name) xmlFree(name);
  if (group) xmlFree(group);
  
  if (object)
  {
    world->InsertObject(object);
  }
  
  return object;
}


CCar::CCar(CWorld* World, float x, float y, float heading, std::string name,
           std::string aMaCIGroup, bool driverCam, bool headCam, bool boomCam,
           float boomFOV, float boomLength, float boomHeight)
:CObject(World,NULL),
name(name),
MaCIGroup(World->GetBaseMaCIGroup()+"."+aMaCIGroup),
gimi(),
mcs(&gimi),
SpeedCtrlServer(&mcs,0),
PositionServer(&mcs,0),
boom(NULL),
boomLength(boomLength),
boomHeight(boomHeight),
speedTimer(0)
{
  float z=1.1;    // starting height of Avant

  y=-y;
 
  mcs.SetGroupName(name);
  
  //materials
  wheelMaterial.look.color[0]=0.2;
  wheelMaterial.look.color[1]=0.2;
  wheelMaterial.look.color[2]=0.2;
  wheelMaterial.density=300.0;

  wheelMaterial.surface.mu=3000.0;

  bodyMaterial.look.color[0]=0.8;
  bodyMaterial.look.color[1]=0.9;
  bodyMaterial.look.color[2]=0.1;
  bodyMaterial.density=BODY_DENSITY;
  
  bodyMaterial.surface.mu=1000.0;
  
  boomMaterial.look.color[0]=0.4;
  boomMaterial.look.color[1]=0.4;
  boomMaterial.look.color[2]=0.4;
  
  Parts.push_back(body=new CBox(BODY_WIDTH, BODY_LENGTH,
                  BODY_HEIGHT,this,this->SpaceID,0,&bodyMaterial,false));
  
  if (boomCam)
  Parts.push_back(boom=new CBox(BOOM_WIDTH, boomLength,
                  BOOM_HEIGHT,this,this->SpaceID,0,&boomMaterial,false));
  
  Parts.push_back(roof=new CBox(ROOF_WIDTH, ROOF_LENGTH,
                  ROOF_HEIGHT,this,this->SpaceID,body->BodyID,&bodyMaterial,false));
  
  Parts.push_back(frontRightBumper=new CBox(BUMPER_WIDTH, FRONT_BUMPER_LENGTH,
                  BUMPER_HEIGHT,this,this->SpaceID,body->BodyID,&bodyMaterial,false));
  
  Parts.push_back(frontLeftBumper=new CBox(BUMPER_WIDTH, FRONT_BUMPER_LENGTH,
                  BUMPER_HEIGHT,this,this->SpaceID,body->BodyID,&bodyMaterial,false));
  
  Parts.push_back(rearRightBumper=new CBox(BUMPER_WIDTH, REAR_BUMPER_LENGTH,
                  BUMPER_HEIGHT,this,this->SpaceID,body->BodyID,&bodyMaterial,false));
  
  Parts.push_back(rearLeftBumper=new CBox(BUMPER_WIDTH, REAR_BUMPER_LENGTH,
                  BUMPER_HEIGHT,this,this->SpaceID,body->BodyID,&bodyMaterial,false));
  
  
  Parts.push_back(crossBlock=new CBox(TRACK+WHEEL_WIDTH, WHEELBASE-WHEEL_RADIUS-0.4,
                  BODY_HEIGHT,this,this->SpaceID,body->BodyID,&bodyMaterial,false));
  
  Parts.push_back(topBlock=new CBox(TOP_BLOCK_WIDTH, TOP_BLOCK_LENGTH,
                  TOP_BLOCK_HEIGHT,this,this->SpaceID,body->BodyID,&bodyMaterial,false));
  
  Parts.push_back(frontRightWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,
                  this,this->SpaceID,0,&wheelMaterial,false));
  
  Parts.push_back(frontLeftWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,
                  this,this->SpaceID,0,&wheelMaterial,false));
  
  Parts.push_back(rearRightWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,
                  this,this->SpaceID,0,&wheelMaterial,false));
   
  Parts.push_back(rearLeftWheel=new CCylinder(WHEEL_RADIUS,WHEEL_WIDTH,
                  this,this->SpaceID,0,&wheelMaterial,false));
   
  
  dReal pos[3]={0,-boomLength/2.0,boomHeight};
  dReal pos2[3]={-0.3,0.6,0.7};
  dReal pos3[3]={0.0,1.4,0.6};
  dMatrix3 rot;
  //dRSetIdentity(rot);
  //dRFrom2Axes(rot, 0,1,-0.5, -1,0,0);
  dRFrom2Axes(rot, 0,boomLength,-boomHeight, -1,0,0);
  
  if (boomCam)
  Children.push_back(boomCamera=new CCamera(World,this,boom->BodyID,
                     pos,rot,640,480,boomFOV,&gimi,&mcs,"BoomCamera", "ServoCtrlBoomCam" ));
  
  dRFrom2Axes(rot, 0,1,0, -1,0,0);
  
  if (driverCam)
  Children.push_back(driverCamera=new CCamera(World,this,body->BodyID,
                     pos2,rot,640,480,70,&gimi,&mcs,"DriverCamera", "ServoCtrlDriverCam" ));
  
  if (headCam)
    Children.push_back(headCamera=new CCamera(World,this,body->BodyID,
                       pos3,rot,640,480,70,&gimi,&mcs,"HeadCamera", "ServoCtrlHeadCam" ));
  
  setPose(x,y,DegToRad(heading));
  
  
  
  
  //pose set, joints can be created and set
  
  if (boomCam)
  {
    boomJoint=dJointCreateHinge(this->WorldID,0);
    dJointAttach(boomJoint, body->BodyID, boom->BodyID);
    dJointSetHingeAxis(boomJoint,0,1,0);
  }
    
  frontRightJoint=dJointCreateHinge2(this->WorldID,0);
  frontLeftJoint=dJointCreateHinge2(this->WorldID,0);
  rearRightJoint=dJointCreateHinge2(this->WorldID,0);
  rearLeftJoint=dJointCreateHinge2(this->WorldID,0);
  
  dJointAttach(frontRightJoint, body->BodyID, frontRightWheel->BodyID);
  dJointAttach(frontLeftJoint, body->BodyID, frontLeftWheel->BodyID);
  dJointAttach(rearRightJoint, body->BodyID, rearRightWheel->BodyID);
  dJointAttach(rearLeftJoint, body->BodyID, rearLeftWheel->BodyID);
  
  const dReal *P,*RR;
  //set joint anchors to middle of wheels
  P=dBodyGetPosition(frontRightWheel->BodyID);
  dJointSetHinge2Anchor(frontRightJoint,P[0],P[1],P[2]);
  P=dBodyGetPosition(frontLeftWheel->BodyID);
  dJointSetHinge2Anchor(frontLeftJoint,P[0],P[1],P[2]);
  
  P=dBodyGetPosition(rearRightWheel->BodyID);
  dJointSetHinge2Anchor(rearRightJoint,P[0],P[1],P[2]);
  P=dBodyGetPosition(rearLeftWheel->BodyID);
  dJointSetHinge2Anchor(rearLeftJoint,P[0],P[1],P[2]);
  
  dReal d[3]={0,0,1};
  dReal a[3];
  RR=dBodyGetRotation(frontRightWheel->BodyID);
  RotateVector(RR,d,a);
  dJointSetHinge2Axis2(frontRightJoint,a[0],a[1],a[2]);
  dJointSetHinge2Axis1(frontRightJoint,0,1,0);
  
  dJointSetHinge2Axis2(frontLeftJoint,a[0],a[1],a[2]);
  dJointSetHinge2Axis1(frontLeftJoint,0,1,0);
  
  dJointSetHinge2Axis2(rearRightJoint,a[0],a[1],a[2]);
  dJointSetHinge2Axis1(rearRightJoint,0,1,0);
  
  dJointSetHinge2Axis2(rearLeftJoint,a[0],a[1],a[2]);
  dJointSetHinge2Axis1(rearLeftJoint,0,1,0);
  
  
  dBodySetFiniteRotationMode(frontRightWheel->BodyID,1);
  dBodySetFiniteRotationAxis(frontRightWheel->BodyID,a[0],a[1],a[2]);
  dBodySetFiniteRotationMode(frontLeftWheel->BodyID,1);
  dBodySetFiniteRotationAxis(frontLeftWheel->BodyID,a[0],a[1],a[2]);
  
  dBodySetFiniteRotationMode(rearRightWheel->BodyID,1);
  dBodySetFiniteRotationAxis(rearRightWheel->BodyID,a[0],a[1],a[2]);
  dBodySetFiniteRotationMode(rearLeftWheel->BodyID,1);
  dBodySetFiniteRotationAxis(rearLeftWheel->BodyID,a[0],a[1],a[2]);
  
  
  
//   dJointSetHinge2Axis1();
//   dJointSetHinge2Axis2();
  
//   dJointSetHinge2Param( frontRightJoint, dParamLoStop, 0 );
//   dJointSetHinge2Param( frontRightJoint, dParamHiStop, 0.01 );
//   dJointSetHinge2Param( frontLeftJoint, dParamLoStop, 0 );
//   dJointSetHinge2Param( frontLeftJoint, dParamHiStop, 0.01 );
//   
  dJointSetHinge2Param( rearRightJoint, dParamLoStop, 0 );
  dJointSetHinge2Param( rearRightJoint, dParamHiStop, 0.001 );
  dJointSetHinge2Param( rearLeftJoint, dParamLoStop, 0 );
  dJointSetHinge2Param( rearLeftJoint, dParamHiStop, 0.001 );
  
  
  
  
}

CCar::~CCar()
{
  
}

void CCar::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{
  
}

void CCar::Activate()
{
  connectGIMI();
  
  SpeedCtrlServer.SetInterfaceInstanceName("Motion");
  SpeedCtrlServer.SetSpeedLimits(3, 3, 1, 0.5, 0.5, 0.5);
  //SpeedCtrlServer.SetSpeedLimits(float aMaxSpeed, float aMaxAngularSpeed, float aMaxAcceleration,
    //                      float aMinSpeed, float aMinAngularSpeed, float aMinAcceleration);
  MaCI::EMaCIError e=SpeedCtrlServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open SpeedCtrlServer");
  } else {
    SpeedCtrlServer.SetSpeedLimits(MAX_SPEED_MPS, STEER_ANGLE_DEGREES_MAX*M_PI/180.0, 1,
                                   -MAX_REVERSE_SPEED_MPS, -STEER_ANGLE_DEGREES_MAX*M_PI/180.0, 1);
  }
  
  e=PositionServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open PositionServer");
  }
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Activate();
  }
  
}

void CCar::Update(float StepSize)
{
  
  //finite mode for joints
  
  //erp and cfm for suspension
  float suspERP, suspCFM;
  suspERP=StepSize*SUSPENSION_SPRING/(StepSize*SUSPENSION_SPRING+SUSPENSION_DAMP);
  suspCFM=1/(StepSize*SUSPENSION_SPRING+SUSPENSION_DAMP);
  
  dJointSetHinge2Param( frontRightJoint, dParamSuspensionCFM, suspCFM);
  dJointSetHinge2Param( frontRightJoint, dParamSuspensionERP, suspERP);
  dJointSetHinge2Param( frontLeftJoint, dParamSuspensionCFM, suspCFM);
  dJointSetHinge2Param( frontLeftJoint, dParamSuspensionERP, suspERP);
  dJointSetHinge2Param( rearRightJoint, dParamSuspensionCFM, suspCFM);
  dJointSetHinge2Param( rearRightJoint, dParamSuspensionERP, suspERP);
  dJointSetHinge2Param( rearLeftJoint, dParamSuspensionCFM, suspCFM);
  dJointSetHinge2Param( rearLeftJoint, dParamSuspensionERP, suspERP);
  
  dVector3 a;
  dJointGetHinge2Axis2(frontRightJoint, a);
  dBodySetFiniteRotationAxis(frontRightWheel->BodyID,a[0],a[1],a[2]);
  dJointGetHinge2Axis2(frontLeftJoint, a);
  dBodySetFiniteRotationAxis(frontLeftWheel->BodyID,a[0],a[1],a[2]);
  
  dJointGetHinge2Axis2(rearRightJoint, a);
  dBodySetFiniteRotationAxis(rearRightWheel->BodyID,a[0],a[1],a[2]);
  dJointGetHinge2Axis2(rearLeftJoint, a);
  dBodySetFiniteRotationAxis(rearLeftWheel->BodyID,a[0],a[1],a[2]);
  
  //TODO: sivusuuntaisen kitkan muutos renkaisiin niiden pyoriessa
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Update(StepSize);
  }
  
  // Driving speed control
  SpeedCtrl();

  
  speed=dJointGetHinge2Angle2Rate(frontRightJoint)*WHEEL_RADIUS;
  
  //send current speed back
  if (speedTimer>=SPEED_INTERVAL)
  {
    speedTimer=0;
    MaCI::SpeedCtrl::CSpeedCtrlData d;
    d.CreateInternalBinBag();
    
    d.SetCommand(MaCI::SpeedCtrl::TCommand(MaCI::SpeedCtrl::KEventSpeed));
    d.SetSpeed(MaCI::SpeedCtrl::TSpeed(speed,0,0));
    
    SpeedCtrlServer.SendSpeedEvent(d);
  }
  speedTimer+=StepSize;
  
}

void CCar::SpeedCtrl()
{
  if (SpeedCtrlServer.WaitCommand(SpeedCtrlData,0)==MaCI::KMaCIOK)
  {
    const MaCI::SpeedCtrl::TCommand *c=SpeedCtrlData.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c)
      switch (c->cmd)
    {
      case MaCI::SpeedCtrl::KCommandSetSpeed:
        s=SpeedCtrlData.GetSpeed();
        if (s)
        {
//          dPrint(1,"SET SPEED-command!");
          if (s->speed>=0)
          {
//            dPrint(1,"  throttle %f",fabs(s->speed/MAX_SPEED_MPS));
            throttle(fabs(s->speed/MAX_SPEED_MPS));
          } else {
//            dPrint(1,"  reverse %f",fabs(s->speed/MAX_REVERSE_SPEED_MPS));
            reverse(fabs(s->speed/MAX_REVERSE_SPEED_MPS));
          }
//          dPrint(1,"  steer %f",s->angularSpeed);
          steer(s->angularSpeed);
        }
        break;
      case MaCI::SpeedCtrl::KCommandSetStop:
        dPrint(1,"STOP-command!");
        brake(1.0);
        break;
      case MaCI::SpeedCtrl::KCommandTimeout:
        idle();
        break;
    }
  } else {
    //idle();
  }
  
  
}

void CCar::steer(float angle)
{
  //calculate wheel angles according to ackermann steering
  float right, left;
  
  if (angle<0)
  {
    right=angle;
    left=-atan(WHEELBASE / (WHEELBASE/tan(-right)+ TRACK) );
  } else {
    left=angle;
    right=atan(WHEELBASE / (WHEELBASE/tan(left)+ TRACK) );
  }
  
  //steer  
  //ode doesn't have servo joints so implement it
  dReal Gain = 6.0;
  dReal MaxForce = 10000;
  
  dReal position = dJointGetHinge2Angle1(frontRightJoint);
  dReal desiredPosition = -right;
  dReal error = position - desiredPosition;

  dReal velocity = -error * Gain;

  dJointSetHinge2Param(frontRightJoint, dParamFMax, MaxForce);
  dJointSetHinge2Param(frontRightJoint, dParamVel, velocity);
  
  position = dJointGetHinge2Angle1(frontLeftJoint);
  desiredPosition = -left;
  error = position - desiredPosition;

  velocity = -error * Gain;
  
  dJointSetHinge2Param(frontLeftJoint, dParamFMax, MaxForce);
  dJointSetHinge2Param(frontLeftJoint, dParamVel, velocity);
  
  //turn boom camera
  position = dJointGetHingeAngle(boomJoint);
  desiredPosition = BOOM_TURNING_RATIO*angle;
  error = position - desiredPosition;
  
  Gain=5.0;
  
  velocity = -error * Gain;
  
  dJointSetHingeParam(boomJoint, dParamFMax, MaxForce);
  dJointSetHingeParam(boomJoint, dParamVel, velocity);
}

void CCar::throttle(float power)
{
  //throttle
  dJointSetHinge2Param( frontRightJoint, dParamVel2, MAX_SPEED_MPS/WHEEL_RADIUS*power);
  dJointSetHinge2Param( frontLeftJoint, dParamVel2, MAX_SPEED_MPS/WHEEL_RADIUS*power);
  
  dJointSetHinge2Param( frontRightJoint, dParamFMax2, MAX_TORQUE*power);
  dJointSetHinge2Param( frontLeftJoint, dParamFMax2, MAX_TORQUE*power);
}

void CCar::reverse(float power)
{
  //throttle
  dJointSetHinge2Param( frontRightJoint, dParamVel2, -MAX_REVERSE_SPEED_MPS/WHEEL_RADIUS*power);
  dJointSetHinge2Param( frontLeftJoint, dParamVel2, -MAX_REVERSE_SPEED_MPS/WHEEL_RADIUS*power);
  
  dJointSetHinge2Param( frontRightJoint, dParamFMax2, MAX_TORQUE*power);
  dJointSetHinge2Param( frontLeftJoint, dParamFMax2, MAX_TORQUE*power);
}

void CCar::brake(float power)
{
  dPrint(1,"BRAKE!!");
  dJointSetHinge2Param( frontRightJoint, dParamVel2, 0);
  dJointSetHinge2Param( frontLeftJoint, dParamVel2, 0);
  
  dJointSetHinge2Param( frontRightJoint, dParamFMax2, MAX_BRAKE_POWER*power);
  dJointSetHinge2Param( frontLeftJoint, dParamFMax2, MAX_BRAKE_POWER*power);
}

void CCar::idle()
{
  brake(0);
  steer(0);
}

void CCar::Draw()
{
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Draw();
  }

  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    (*i)->Draw(); 
  }
  
  /**************************************/
  /*
  dReal M[16];
  
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  
  frontRightWheel->GetBodyTransMatrix(M);
  glMultMatrixf(M);
  
  glBegin(GL_LINES);
    //x
  glColor3f(1,0,0);
  glVertex3f(0,0,0);
  glVertex3f(1,0,0);
    //z
  glColor3f(0,0,1);
  glVertex3f(0,0,0);
  glVertex3f(0,1,0);
    //y
  glColor3f(0,1,0);
  glVertex3f(0,0,0);
  glVertex3f(0,0,-1);
  glEnd();
//   
  glPopMatrix();
  */
}


void CCar::setPose(float x, float y, float heading)
{
   
  dMatrix3 R,r;

  heading=heading+90.0*M_PI/180.0;
  
  dRFrom2Axes(R,cos(heading), 0, sin(heading),
              sin(heading), 0, -cos(heading));
  dBodySetRotation(body->BodyID,R);
  dBodySetPosition(body->BodyID,x,GROUND_CLEARANCE+BODY_HEIGHT/2.0,y);

  
  dRFromAxisAndAngle(r,0,0,1,-0.15);
  dGeomSetOffsetRotation(frontRightBumper->GeomID,r);
  dGeomSetOffsetPosition(frontRightBumper->GeomID, BUMPER_X, FRONT_BUMPER_Y, 0);
  
  dRFromAxisAndAngle(r,0,0,1,0.15);
  dGeomSetOffsetRotation(frontLeftBumper->GeomID,r);
  dGeomSetOffsetPosition(frontLeftBumper->GeomID, -BUMPER_X, FRONT_BUMPER_Y, 0);
  
  dRFromAxisAndAngle(r,0,0,1,0.15);
  dGeomSetOffsetRotation(rearRightBumper->GeomID,r);
  dGeomSetOffsetPosition(rearRightBumper->GeomID, BUMPER_X, REAR_BUMPER_Y, 0);
  
  dRFromAxisAndAngle(r,0,0,1,-0.15);
  dGeomSetOffsetRotation(rearLeftBumper->GeomID,r);
  dGeomSetOffsetPosition(rearLeftBumper->GeomID, -BUMPER_X, REAR_BUMPER_Y, 0);
  
  dGeomSetOffsetPosition(topBlock->GeomID, 0, 0, BODY_HEIGHT/2.0+TOP_BLOCK_HEIGHT/2.0);
  
  dGeomSetOffsetPosition(roof->GeomID, 0, ROOF_Y, ROOF_Z);
   
  float p[3], pp[3];
  
  if (boom) 
  {
    dBodySetRotation(boom->BodyID,R);
    
    p[0]=0;
    p[2]=GROUND_CLEARANCE+BODY_HEIGHT/2.0+ROOF_Z+ROOF_HEIGHT/2.0+0.1;
    p[1]=-boomLength/2.0;
  
    RotateVector(R,p,pp);
    dBodySetPosition(boom->BodyID,x+pp[0],pp[1],y+pp[2]);
    

//     dBodySetPosition(boom->BodyID,sin(heading)*-BOOM_LENGTH/2.0+x,
//                      GROUND_CLEARANCE+BODY_HEIGHT/2.0+ROOF_Z+ROOF_HEIGHT/2.0+0.1,
//                      cos(heading)*-BOOM_LENGTH/2.0+y);
  }
  
  
  
  p[0]=TRACK/2.0;
  p[2]=WHEEL_RADIUS;
  p[1]=FRONT_WHEELS_Y;
  
  RotateVector(R,p,pp);
  dBodySetPosition(frontRightWheel->BodyID,x+pp[0],pp[1],y+pp[2]);
      
  p[0]=-TRACK/2.0;
  p[1]=FRONT_WHEELS_Y;
  
  RotateVector(R,p,pp);
  dBodySetPosition(frontLeftWheel->BodyID,x+pp[0],pp[1],y+pp[2]);
  
  p[0]=TRACK/2.0;
  p[1]=REAR_WHEELS_Y;
  
  RotateVector(R,p,pp);
  dBodySetPosition(rearRightWheel->BodyID,x+pp[0],pp[1],y+pp[2]);
  
  p[0]=-TRACK/2.0;
  p[1]=REAR_WHEELS_Y;
  
  RotateVector(R,p,pp);
  dBodySetPosition(rearLeftWheel->BodyID,x+pp[0],pp[1],y+pp[2]);
  
      /*
  dBodySetPosition(frontRightWheel->BodyID, 
                   x+cos(-heading)*TRACK/2.0+sin(-heading)*FRONT_WHEELS_Y,
                   WHEEL_RADIUS, 
                   y+sin(-heading)*TRACK/2.0+cos(-heading)*FRONT_WHEELS_Y);
  
  dBodySetPosition(frontLeftWheel->BodyID, 
                   x-cos(-heading)*TRACK/2.0+sin(-heading)*FRONT_WHEELS_Y,
                   WHEEL_RADIUS, 
                   y-sin(-heading)*TRACK/2.0+cos(-heading)*FRONT_WHEELS_Y);
  
  dBodySetPosition(rearRightWheel->BodyID, 
                   x+cos(-heading)*TRACK/2.0+sin(-heading)*REAR_WHEELS_Y,
                   WHEEL_RADIUS, 
                   y+sin(-heading)*TRACK/2.0+cos(-heading)*REAR_WHEELS_Y);
  
  dBodySetPosition(rearLeftWheel->BodyID, 
                   x-cos(-heading)*TRACK/2.0+sin(-heading)*REAR_WHEELS_Y,
                   WHEEL_RADIUS, 
                   y-sin(-heading)*TRACK/2.0+cos(-heading)*REAR_WHEELS_Y);
  */

  dRSetIdentity(R);
  dRFromAxisAndAngle(R,0,1,0,-heading+M_PI/2.0);
  dBodySetRotation(frontRightWheel->BodyID,R);
  dBodySetRotation(frontLeftWheel->BodyID,R);
  
  dBodySetRotation(rearRightWheel->BodyID,R);
  dBodySetRotation(rearLeftWheel->BodyID,R);

  
}

void CCar::connectGIMI()
{
  SimWorld->ConnectGimi(&gimi,name);
}


